﻿using UnityEngine;
using System.Collections;

namespace SG
{
    public static class QuaternionExtension
    {
        public static Quaternion FromAxes(Vector3 xaxis, Vector3 yaxis, Vector3 zaxis)
        {
            Matrix4x4 kRot = new Matrix4x4();

            kRot[0, 0] = xaxis.x;
            kRot[1, 0] = xaxis.y;
            kRot[2, 0] = xaxis.z;

            kRot[0, 1] = yaxis.x;
            kRot[1, 1] = yaxis.y;
            kRot[2, 1] = yaxis.z;

            kRot[0, 2] = zaxis.x;
            kRot[1, 2] = zaxis.y;
            kRot[2, 2] = zaxis.z;

            Quaternion newQuat = FromRotationMatrix(kRot);
            return newQuat;
        }

        //-----------------------------------------------------------------------
        public static Quaternion FromRotationMatrix(Matrix4x4 kRot)
        {
            // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
            // article "Quaternion Calculus and Fast Animation".
            Quaternion newQuat = new Quaternion();

            float fTrace = kRot[0, 0] + kRot[1, 1] + kRot[2, 2];
            float fRoot;

            if (fTrace > 0.0)
            {
                // |w| > 1/2, may as well choose w > 1/2
                fRoot = Mathf.Sqrt(fTrace + 1.0f);  // 2w
                newQuat.w = 0.5f * fRoot;
                fRoot = 0.5f / fRoot;  // 1/(4w)
                newQuat.x = (kRot[2, 1] - kRot[1, 2]) * fRoot;
                newQuat.y = (kRot[0, 2] - kRot[2, 0]) * fRoot;
                newQuat.z = (kRot[1, 0] - kRot[0, 1]) * fRoot;
            }
            else
            {
                // |w| <= 1/2
                int[] s_iNext = new int[3] { 1, 2, 0 };
                int i = 0;
                if (kRot[1, 1] > kRot[0, 0])
                    i = 1;
                if (kRot[2, 2] > kRot[i, i])
                    i = 2;
                int j = s_iNext[i];
                int k = s_iNext[j];

                fRoot = Mathf.Sqrt(kRot[i, i] - kRot[j, j] - kRot[k, k] + 1.0f);
                float[] apkQuat = null;
                RAMAssignment(fRoot, kRot, i, j, k, out apkQuat);

                newQuat.x = apkQuat[0];
                newQuat.y = apkQuat[1];
                newQuat.z = apkQuat[2];
                newQuat.w = apkQuat[3];
                //float* apkQuat[3] = { &x, &y, &z };
                //*apkQuat[i] = 0.5f * fRoot;
                //fRoot = 0.5f / fRoot;
                //w = (kRot[k, j] - kRot[j, k]) * fRoot;
                //*apkQuat[j] = (kRot[j, i] + kRot[i, j]) * fRoot;
                //*apkQuat[k] = (kRot[k, i] + kRot[i, k]) * fRoot;
            }

            return newQuat;
        }

        /// <summary>
        /// 地址赋值
        /// </summary>
        /// <param name="fRoot"></param>
        /// <param name="kRot"></param>
        /// <param name="i"></param>
        /// <param name="j"></param>
        /// <param name="k"></param>
        /// <param name="apkQuat"></param>
        public static void RAMAssignment (float fRoot, Matrix4x4 kRot, int i, int j, int k, out float[] apkQuat)
        {
            apkQuat = new float[4];

            apkQuat[i] = 0.5f * fRoot;
            fRoot = 0.5f / fRoot;
            apkQuat[3] = (kRot[k, j] - kRot[j, k]) * fRoot;
            apkQuat[j] = (kRot[j, i] + kRot[i, j]) * fRoot;
            apkQuat[k] = (kRot[k, i] + kRot[i, k]) * fRoot;
        }

        public static void normalise(this Quaternion source)
        {
            float fLength = source.w * source.w + source.x * source.x + source.y * source.y + source.z * source.z;
            float factor = 1.0f / Mathf.Sqrt(fLength);

            source = source.scaler(factor);
        }

        public static Quaternion scaler(this Quaternion source, float fScalar)
        {
            Quaternion newQuat = new Quaternion(fScalar * source.w, fScalar * source.x, fScalar * source.y, fScalar * source.z);
            return newQuat;
        }


    }
}

